/*********************************************************************  
Create Robot  
*********************************************************************/


/* adapt the following settings as necessary */

#define LIGHT_ANALOG 0 // port for analog light sensor
#define RUN_FOR 120 // how long to run in seconds

#define S1_HOME_POSITION 704 // servo 1 home position
#define S3_HOME_POSITION 1144 // servo 3 home position

#define S1_UP_POSITION 324 // servo 1 up position
#define S3_UP_POSITION 1524 // servo 3 up position

#define S1_DOWN_POSITION 924 // servo 1 down position
#define S3_DOWN_POSITION 924 // servo 3 down position

#define TRACK_OFFSET_RIGHT 80 // camera right offset
#define TRACK_OFFSET_LEFT 70 // camera left offset

#define WAIT_TIMEOUT 5 // how long to wait before ball assumed lost

/* prototypes */

void beginProgram(void);

// claw
void openClaw(void);
void closeClaw(void);

// arm
void moveArmHome(void);
void moveArmUp(int movementIncrements, int s1UpPosition, int s3UpPosition);
void moveArmDown(int movementIncrements, int s1DownPosition, int s3DownPosition);

// create movement
void moveForward(int speed, int seconds);
void rotateDegrees(int rotationDirection, int degreesToRotate, int rotationSpeed);

// green ball tracking
int findGreenBall(void);
void moveLeftRightToBall(void);
int isBall();

// navigational
void sweepBalls(void);

/* functions */

/**
 * main() function
 *
 * Waits for create connection and enables servos, positions arm home and opens 
 * the claw, waits for light, and runs the beginProgram function before
 * shutting down.
 */
int main()
{	
	while (get_create_mode(1) != 2)
	{
		beep();
		printf("Attempting to connect to the Create\n");
		create_connect();
		sleep(1);
	}
	enable_servos();
	// set_each_analog_state ??
	// create_spot, create_cover
	
	moveArmHome();
	
	openClaw();
	
	wait_for_light(LIGHT_ANALOG);
	run_for(RUN_FOR, beginProgram);
  
	disable_servos();
	create_disconnect();
	
	return 0;
}

/* function definitions */

/**
 * beginProgram() function
 *
 * Our actual robotic program, called from main().
 * 
 * Sweeps balls, then loops finding them, attempting to pick them up within a 
 * timeframe.
 */
void beginProgram(void)
{
	sweepBalls();
	
	while(1)
	{
		moveLeftRightToBall();
		
		int tryAgain = 0;
		int secs = 0;
		while (!isBall(0)) // stop at infrared
		{
			create_drive_direct(-100, -100);
			
			if (secs >= WAIT_TIMEOUT)
			{
				tryAgain = 1;
				break;
			}
			
			sleep(1);
			secs++;
		}
			
		create_stop();
		
		if (tryAgain)
		{
			continue;
		}
			
		closeClaw();
		
		moveArmUp(100, S1_UP_POSITION, S3_UP_POSITION);
	
		/* we have a ball */
	}
}

/**
 * openClaw() function
 *
 * This function sets the claw servo to the open
 * position.
 */
void openClaw(void)
{
	set_servo_position(0, 1728);
	printf("Claw opened\n");
	sleep(1);
}

/**
 * closeClaw() function
 *
 * This function sets the claw servo to the closed position.
 */
void closeClaw(void)
{
	set_servo_position(0, 724);
	printf("Claw closed\n");
	sleep(1);
}

/**
 * moveArmHome() function
 *
 * This function moves the arm to it's home position.
 */
void moveArmHome(void)
{
	set_servo_position(1, S1_HOME_POSITION);
	set_servo_position(3, S3_HOME_POSITION);
	printf("Arm moved home\n");
	sleep(1);
}

/**
 * moveArmUp() function
 *
 * This function moves the arm up to a certain position in increments over
 * time.
 *
 * @param int movementIncrements The interval of increments every 1/10th/second
 * @param int s1UpPosition The servo 1 up position
 * @param int s3UpPosition The servo 3 up position
 */
void moveArmUp(int movementIncrements, int s1UpPosition, int s3UpPosition)
{	
	int continueLoop;
	
	int notCloseToUse = 1;
	while(notCloseToUse)
	{
		continueLoop = 0;
		
		if (get_servo_position(1) >= s1UpPosition + movementIncrements)
		{
			set_servo_position(1, get_servo_position(1) - movementIncrements);
			continueLoop = 1;
		}
		
		
		if (get_servo_position(3) <= s3UpPosition - movementIncrements)
		{
			set_servo_position(3, get_servo_position(3) + movementIncrements);
			continueLoop = 1;
		}
		
		notCloseToUse = continueLoop;
		
		sleep(0.1);
	}
	
	set_servo_position(1, s1UpPosition);
	set_servo_position(3, s3UpPosition);
	printf("Arm is raised\n");
	sleep(1);
}

/**
 * moveArmDown() function
 *
 * This function moves the arm down to a certain position in increments over
 * time.
 *
 * @param int movementIncrements The interval of increments every 1/10th/second
 * @param int s1DownPosition The servo 1 up position
 * @param int s3DownPosition The servo 3 up position
 */
void moveArmDown(int movementIncrements, int s1DownPosition, int s3DownPosition)
{
	int continueLoop;
	
	int notCloseToUse = 1;
	while(notCloseToUse)
	{
		continueLoop = 0;
		
		if (get_servo_position(1) <= s1DownPosition - movementIncrements)
		{
			set_servo_position(1, get_servo_position(1) + movementIncrements);
			continueLoop = 1;
		}
		
		if (get_servo_position(3) >= s3DownPosition + movementIncrements)
		{
			set_servo_position(3, get_servo_position(3) - movementIncrements);
			continueLoop = 1;
		}
		
		notCloseToUse = continueLoop;
		
		sleep(0.1);
	}
			
	set_servo_position(1, s1DownPosition);
	set_servo_position(3, s3DownPosition);
	printf("Arm is lowered\n");
	sleep(1);
}

/**
 * moveForward() function
 *
 * This function moves the create forward at a certain speed for a specified
 * number of seconds.
 *
 * @param int speed The speed to drive, 20-500 (mm/s)
 * @param int seconds The seconds to drive for
 */
void moveForward(int speed, int seconds)
{
	printf("Moving forward\n");
	
	int secs = 0;
	while (secs <= seconds)
	{
		create_drive_direct(-speed, -speed);
		sleep(1);
		secs++;
	}
	
	create_stop();
}

/**
 * rotateDegrees() function
 *
 * This function rotates a certain direction for a certain number of degrees at
 * a specified speed.
 *
 * @param int rotationDirection The direction to rotate, 0 cw, 1 ccw
 * @param int degreesToRotate The degrees to rotate
 * @param int rotationSpeed The speed of rotation, 20-500 (mm/s)
 */
void rotateDegrees(int rotationDirection, int degreesToRotate, int rotationSpeed)
{
	printf("Rotating %d degrees\n", degreesToRotate);
	
	switch (rotationDirection)
	{
		case 0:
			while (get_create_total_angle(0.001) > -degreesToRotate) 
			{
				create_drive_direct(-rotationSpeed, rotationSpeed);
			}
			break;
		case 1:
			while (get_create_total_angle(0.001) < degreesToRotate)
			{
				create_drive_direct(rotationSpeed, -rotationSpeed);
			}
			break;
	}
	
	create_stop();
	set_create_total_angle(0);
	
	printf("Done rotating\n");
	sleep(1);
}

/**
 * findGreenBall() function
 *
 * This function returns the "X" coordinate of the largest green blob.
 *
 * @return int "X" coordinate of largest green blob
 */
int findGreenBall(void)
{	
	if (track_is_new_data_available())
	{
		track_update();
	}
	
	printf("The ball is at %d, %d\n", track_x(0, 0), track_y(0, 0));
	
	return track_x(0, 0);
}

/**
 * moveLeftRightToBall() function
 *
 * This function attempts to move the robot so that a green ball is centered
 * in front of the claw in a way that picking it up should only require forward
 * motion.
 */
void moveLeftRightToBall(void)
{
	int greenBall = findGreenBall();
	
	while (greenBall > TRACK_OFFSET_RIGHT || greenBall < TRACK_OFFSET_LEFT)
	{
		if (greenBall > TRACK_OFFSET_RIGHT)
		{
			create_drive_direct(-50, 50);
			printf("Adjusting right\n");
		}
		
		else if (greenBall < TRACK_OFFSET_LEFT)
		{
			create_drive_direct(50, -50);
			printf("Adjusting left\n");
		}
		
		greenBall = findGreenBall();
	}
	
	create_stop();
	printf("The ball is centered\n");
}

/**
 * isBall() function
 *
 * This function uses an analog sensor to determine whether or not a ball is
 * in the claw.
 *
 * @param int analogPort The port of the top-hat analog
 * @return int Returns 1 if a ball was detected, 0 if not
 */
int isBall(int analogPort)
{
	if (analog10(analogPort) < 500)
	{
		printf("Ball is in position\n");
		return 1;
	}
	
	return 0;
}

/**
 * sweepBalls() function
 *
 * This function rotates to sweep the balls out of their pile, then returns
 * to the inital position.
 */
void sweepBalls(void) 
{
	moveArmUp(100, S1_UP_POSITION, S3_UP_POSITION);
	rotateDegrees(0, 90, 200);
	moveArmDown(100, S1_DOWN_POSITION, S3_DOWN_POSITION);
	rotateDegrees(1, 90, 500);
	moveArmUp(100, S1_UP_POSITION, S3_UP_POSITION);
	sleep(1);
}